/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "vrepobject.h"
#include <QDebug>

#include <stdio.h>
#include <stdlib.h>
#include "extIk.h"
extern "C" {
#include "extApi.h"
}


#if D_CHECK_SYNC_TIME
#include <sys/time.h>
#include <unistd.h>
#endif


VrepObject::VrepObject(QObject *parent) :
    QObject(parent)
{
    isRunDemo=false;
    setChanged=false;
    getChanged=false;
    for(int i=0;i<D_CONST_AXIS_MAX;i++)
    {
        setPos[i]=0.0;
        getPos[i]=0.0;
    }
    axisNum=0;
    robotChoose=-1;
}



void VrepObject::initVrepRobot( int clientID, int robot_choose )
{
    qDebug()<< "robot_choose:"<< robot_choose;

#if  D_HAS_IK_MODE
    QFile file( "lbr_iiwa_7_r800-2.ik" );
    if(!file.open( QIODevice::ReadOnly ))
    {
        qDebug()<<"The kinematic content file 'lbr.ik' could not be read!\n";
        return -1;
    }
    int dataLength=  file.size();
    unsigned char* data = new unsigned char[dataLength];
    file.read( (char*)data , dataLength);
    file.close();
    file.flush();//必须加，如果不加，可能关机的时候丢失文件内容。

    // Initialize the embedded robot model:
    int robotHandle1=simEmbLaunch(); //初始化机器人矩阵
    simEmbStart(data,dataLength);
#endif

    simxChar* objname[ D_CONST_AXIS_MAX ];
    int z_count=0;
    switch(robot_choose){
    case IRB4600: axisNum = 6 ;
        objname[z_count++] = "IRB4600_joint1";
        objname[z_count++] = "IRB4600_joint2";
        objname[z_count++] = "IRB4600_joint3";
        objname[z_count++] = "IRB4600_joint4";
        objname[z_count++] = "IRB4600_joint5";
        objname[z_count++] = "IRB4600_joint6";
        break;
    case UR5: axisNum = 6 ;
        objname[z_count++] = "UR5_joint1";
        objname[z_count++] = "UR5_joint2";
        objname[z_count++] = "UR5_joint3";
        objname[z_count++] = "UR5_joint4";
        objname[z_count++] = "UR5_joint5";
        objname[z_count++] = "UR5_joint6";
        break;
    case LBR_iiwa_7_R800: axisNum = 7 ;
        objname[z_count++]  = "LBR_iiwa_7_R800_joint1";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint2";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint3";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint4";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint5";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint6";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint7";
        break;
    case MTB_Robot: axisNum = 4 ;
        objname[z_count++] = "MTB_axis1";
        objname[z_count++] = "MTB_axis2";
        objname[z_count++] = "MTB_axis3";
        objname[z_count++] = "MTB_axis4";
        break;
    case UARM: axisNum = 4 ;
        objname[z_count++] = "uarm_motor1";
        objname[z_count++] = "uarm_motor2";
        objname[z_count++] = "uarm_motor3";
        objname[z_count++] = "uarm_motor4";
        break;
    default:
        robotChoose=-1;
        displayMsg("not support robot ",robot_choose);
        return;
    }
    robotChoose=robot_choose;
    for(int i=0;i<axisNum; i++ ){
        simxGetObjectHandle(clientID ,objname[i], &obHandles[i], simx_opmode_blocking);
    }

    displayMsg("choose robot :", robot_choose);
}


void VrepObject::initVrepRobot(int clientID, QString robotName )
{
    qDebug()<< "robot_choose:"<< robotName;
    simxChar* objname[ D_CONST_AXIS_MAX ];
    if(robotName.indexOf("MTB_Robot")>=0){
        robotChoose=MTB_Robot;
    }else if(robotName.indexOf("IRB4600")>=0){
        robotChoose=IRB4600;
    }else if(robotName.indexOf("UR5")>=0){
        robotChoose=UR5;
    }else if(robotName.indexOf("LBR_iiwa_7_R800")>=0){
        robotChoose=LBR_iiwa_7_R800;
    }else if(robotName.indexOf("TCR7")>=0){
        robotChoose=TCR7;
    }else if(robotName.indexOf("uarm")>=0){
        robotChoose=UARM;
    }else if(robotName.indexOf("5AxisWristAcRobot")>=0){
        robotChoose=E_ROBOT_5AxisWristAcRobot;
    }
    else{
        robotChoose=-1;
    }
    int z_count=0;

    switch(robotChoose){
    case IRB4600: axisNum = 6 ;
        objname[z_count++] = "IRB4600_joint1";
        objname[z_count++] = "IRB4600_joint2";
        objname[z_count++] = "IRB4600_joint3";
        objname[z_count++] = "IRB4600_joint4";
        objname[z_count++] = "IRB4600_joint5";
        objname[z_count++] = "IRB4600_joint6";
        break;
    case UR5: axisNum = 6 ;
        objname[z_count++] = "UR5_joint1";
        objname[z_count++] = "UR5_joint2";
        objname[z_count++] = "UR5_joint3";
        objname[z_count++] = "UR5_joint4";
        objname[z_count++] = "UR5_joint5";
        objname[z_count++] = "UR5_joint6";
        break;
    case LBR_iiwa_7_R800: axisNum = 7 ;
        objname[z_count++]  = "LBR_iiwa_7_R800_joint1";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint2";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint3";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint4";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint5";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint6";
        objname[z_count++]  = "LBR_iiwa_7_R800_joint7";
        break;
    case MTB_Robot: axisNum = 4 ;
        objname[z_count++] = "MTB_axis1";
        objname[z_count++] = "MTB_axis2";
        objname[z_count++] = "MTB_axis3";
        objname[z_count++] = "MTB_axis4";
        break;
    case TCR7: axisNum = 7 ;
        objname[z_count++]  = "J1";
        objname[z_count++]  = "J2";
        objname[z_count++]  = "J3";
        objname[z_count++]  = "J4";
        objname[z_count++]  = "J5";
        objname[z_count++]  = "J6";
        objname[z_count++]  = "J7";
        break;
    case E_ROBOT_5AxisWristAcRobot: axisNum = 5 ;
        objname[z_count++]  = "j1";
        objname[z_count++]  = "j2";
        objname[z_count++]  = "j3";
        objname[z_count++]  = "j4";
        objname[z_count++]  = "j5";
        break;
    case UARM: axisNum = 4 ;
        objname[z_count++] = "uarm_motor1";
        objname[z_count++] = "uarm_motor2";
        objname[z_count++] = "uarm_motor3";
        objname[z_count++] = "uarm_motor4";
        break;
    default:
        axisNum = 0 ;
        robotChoose=-1;
        displayMsg("not support robot: "+robotName, robotChoose);
        return;
    }
    int id = -1;
    bool ok=false;
    int pos=robotName.indexOf("#") ;
    if( pos>0 ){
        id = robotName.right(pos-2).toInt(&ok);
        qDebug()<<"有相同型号的机器人:"<<robotName.right(pos)<<robotName.right(pos-2)<< robotChoose << id;
    }

    if(ok && pos>0 )
    {
        for(int i=0;i<axisNum;i++){
            char dst[120]={0};
            sprintf(dst,"%s%s%d", objname[i],"#",id );
            obHandles[i]=0;
            simxGetObjectHandle(clientID ,dst , &obHandles[i], simx_opmode_blocking);
            if( obHandles[i] == 0 ){
                simxGetObjectHandle(clientID ,dst , &obHandles[i], simx_opmode_blocking);
            }
            if( obHandles[i] == 0 ){
                simxGetObjectHandle(clientID ,dst , &obHandles[i], simx_opmode_blocking);
            }
            qDebug()<<"34s2 vrep机器人:"<< dst << objname[i]<<obHandles[i];
        }
    }else{
        for(int i=0;i<axisNum; i++ ){
            obHandles[i]=0;
            simxGetObjectHandle(clientID ,objname[i], &obHandles[i], simx_opmode_blocking);
            if( obHandles[i] == 0 ){
                simxGetObjectHandle(clientID ,objname[i], &obHandles[i], simx_opmode_blocking);
            }
            if( obHandles[i] == 0 ){
                simxGetObjectHandle(clientID ,objname[i], &obHandles[i], simx_opmode_blocking);
            }
            qDebug()<<"4d56 vrep机器人:" << objname[i]<<obHandles[i];
        }
    }

    displayMsg("choose robot :", robotChoose);
}

void VrepObject::slot_set_pos( int axis, float val )
{
    if( axis < axisNum ){
        setPos[axis]=val;
        setChanged=true;
    } else{
        displayMsg("error: axis!",axis);
    }
}

void VrepObject::slot_set_pos( QVector<float> val )
{
    if( val.size() == axisNum ){
        for(int i=0;i<val.size();i++){
            setPos[i]=val.at(i);
        }
        setChanged=true;
    }else{
        displayMsg("error: axis!",val.size());
    }
}

void VrepObject::slot_get_pos(QVector<float> &val )
{
    if( val.size() == axisNum ){
        //val.resize( axisNum );
        for(int i=0;i<axisNum; i++){
            val[i]=getPos[i];
        }
        getChanged=false;
    }else{
        displayMsg("error: axis!",val.size());
    }

}

void VrepObject::displayMsg( QString msg ,int code )
{
#if 0
    qDebug()<< code << msg;
#else
    emit signal_msg( msg, code );
#endif
}

void VrepObject::test_demo()
{
    if( !isRunDemo ){
        for(int i=0; i< axisNum; i++ ){
            demoPos[i] = getPos[i];
        }
        return;
    }

    static float fsaf= 0.01;
    static bool fanzhuan = false;
    if( demoPos[0]>1.0f ){
        fanzhuan= !fanzhuan;
        //qDebug()<< "getPos[0]:"<<getPos[0];
    }else if(  demoPos[0]<-1.0f ){
        fanzhuan= !fanzhuan;
        // qDebug()<< "getPos[0]:"<<getPos[0];
    }

    for(int i=0; i<axisNum; i++)
    {
        if( !fanzhuan )
            demoPos[i] += fsaf;
        else
            demoPos[i] -= fsaf;
        //  qDebug() <<"getPosxxxx"<< getPos[i];
        // sData.vecData.push_back( getPos[i] );
        setPos[i]=demoPos[i];
    }
    setChanged=true;
}
